#include "aruco_test.h"


namespace aruco_node
{

ArucoTest::ArucoTest(int arucoFormatId):arucoFormatId_(arucoFormatId)
{   
    markerLength = atof(pd.getData("markerLength").c_str());
    markersX = atoi(pd.getData("markersX").c_str());
    markersY = atoi(pd.getData("markersY").c_str());
    markerSaparation = atof(pd.getData("markerSaparation").c_str());

    dict = cv::aruco::getPredefinedDictionary(arucoFormatId_);
    parameters = cv::aruco::DetectorParameters::create();

    // x,y方向标记数,markerLength,间隔,字典
    board  = cv::aruco::GridBoard::create(markersX, markersY, markerLength, markerSaparation, dict);
}

void ArucoTest::create_aruco(int id, int length)
{
    cv::aruco::drawMarker(dict, id, length, markerImage, 1);
    std::string winName = "maker" + std::to_string(id);
    cv::imshow(winName, markerImage);
    cv::waitKey(0);
}

void ArucoTest::create_aruco_id(int id, int length)
{
    cv::aruco::drawMarker(dict, id, length, markerImage, 1);
    std::string winName = "maker" + std::to_string(id);
    cv::imshow(winName, markerImage);
    cv::waitKey(0);
}

void ArucoTest::detete_aruco(cv::Mat& inputImage)
{   
    cv::aruco::detectMarkers(inputImage, dict, markerCorners, markerIds, parameters, rejectedCandidates); 
    cv::aruco::drawDetectedMarkers(inputImage, markerCorners, markerIds, cv::Scalar(0, 255, 0));
    // cv::aruco::drawDetectedMarkers(inputImage, markerCorners, markerIds, cv::Scalar(255, 0, 0));
}


void ArucoTest::save_arurco(int id, int length)
{   
    std::string imgPath = "../data/maker" + std::to_string(id) + ".png";
    cv::imwrite(imgPath, markerImage);
    std::cout << "Save " << length << "*" << length << " ArUco image to " << imgPath << "successfully." << std::endl;
}

void ArucoTest::pose_estimation(cv::Mat& inputImage, CameraConfig& camera_conf, Result_Pose_estimation& result)
{
    
    cv::aruco::detectMarkers(inputImage, dict, markerCorners, markerIds, parameters, rejectedCandidates); 
    
    if(markerIds.size()>0)
    {
        cv::aruco::drawDetectedMarkers(inputImage, markerCorners, markerIds);
        // 第二个参数markerLength边长,单位m
        cv::aruco::estimatePoseSingleMarkers(markerCorners, markerLength, camera_conf.cameraMatrix, camera_conf.distCoeffs, result.rvecs, result.tvecs);
   
        //绘制坐标轴，检查姿态估计结果
        
        for (int i = 0;i < result.rvecs.size();i++)
        {
            //绘制坐标轴，检查姿态估计结果 // 最后一个参数为坐标轴长度单位m
            cv::aruco::drawAxis(inputImage, camera_conf.cameraMatrix, camera_conf.distCoeffs, result.rvecs[i], result.tvecs[i], 0.02);         
        
            std::cout << "The markerIds is " << markerIds[i] << "  pose:\nrvec: = " << result.rvecs[i] << "\n tvec = " << result.tvecs[i] << std::endl;
        }

    } 
}

void ArucoTest::pose_grid_board(cv::Mat& inputImage, CameraConfig& camera_conf, Result_Pose_estimation& result)
{
    cv::aruco::detectMarkers(inputImage, dict, markerCorners, markerIds, parameters, rejectedCandidates); 
    if(markerCorners.size() > 0)
    {
        cv::aruco::drawDetectedMarkers(inputImage, markerCorners, markerIds);
        int boardNum = cv::aruco::estimatePoseBoard(markerCorners, markerIds, board, camera_conf.cameraMatrix, camera_conf.distCoeffs, result.rvec, result.tvec);

        if(boardNum > 0)
            cv::aruco::drawAxis(inputImage, camera_conf.cameraMatrix, camera_conf.distCoeffs, result.rvec, result.tvec, 0.04);
    }
}

void ArucoTest::vector2Mat(cv::Vec3d rvec, cv::Vec3d tvec)
{
    cv::Mat R;
    cv::Rodrigues(rvec, R);
    std::cout << R << std::endl;
}


} // aruco_node
